#include "bt_manager/tf_utils.hpp"
#include <cmath>
#include <tf/LinearMath/Quaternion.h>

namespace Sentry_BT
{
  TransformUtils::TransformUtils()
  {
    ros::NodeHandle nh;
    gimbal_yaw_sub_ = nh.subscribe("/sentry/gimbal_yaw", 10, &TransformUtils::gimbalYawCallback, this);
  }

  void TransformUtils::gimbalYawCallback(const std_msgs::Float32::ConstPtr& msg)
  {
    publishStaticTransform(msg->data);
  }

  void TransformUtils::publishStaticTransform(float yaw)
  {
    tf::StampedTransform transform;
    transform.stamp_ = ros::Time::now() + ros::Duration(1.0);
    transform.frame_id_ = "base_link";
    transform.child_frame_id_ = "gimbal";
    
    transform.setOrigin(tf::Vector3(0.0, 0.0, 0.5));
    tf::Quaternion quat;
    quat.setRPY(-M_PI/2, 0, -M_PI/2 + yaw * M_PI/180.0);
    transform.setRotation(quat);
    
    broadcaster_.sendTransform(transform);
  }

  bool TransformUtils::transformPoseToBaseLink(const geometry_msgs::Pose& input_pose, geometry_msgs::Pose& output_pose)
  {
    try
    {
      geometry_msgs::PoseStamped input_stamped, output_stamped;
      input_stamped.header.frame_id = "gimbal";
      input_stamped.header.stamp = ros::Time(0);
      input_stamped.pose = input_pose;
      
      listener_.waitForTransform("map", "gimbal", ros::Time(0), ros::Duration(1.0));
      listener_.transformPose("map", input_stamped, output_stamped);
      
      output_pose = output_stamped.pose;
      return true;
    }
    catch(tf::TransformException& ex)
    {
      ROS_ERROR("Transform failed: %s", ex.what());
      return false;
    }
  }
}